#include <iostream>
#include <string>
#include "../Lib/MiniBotAlgorithmLib.h"
using namespace std;

/*
*末端笛卡尔空间正逆解Sample
*/
int Sample_CalJointAngleToTerminalCartesian()
{
	float angles[5] = { 20,66,88,20,80 };
	auto space = CalJointAngleToTerminalCartesian(angles);

	cout << "TerminalCartesian:" << endl;
	for (auto i = 0; i < 6; i++)
	{
		cout << space[i] << " ";
	}

	auto angles2 = CalTerminalCartesianToJointAngle(space);

	cout << "JointAngles:" << endl;
	for (auto i = 0; i < 5; i++)
	{
		cout << angles2[i] << " ";
	}

	getchar();
	return 0;
}

